#include "app_tentacle.h"
#include "app_motion.h"
#include "bsp_encoder.h"
#include "app_pid.h"
#include "app.h"
#include "bsp_motor.h"

// 全局触角控制变量
tentacle_control_t g_tentacle_ctrl;

// 轨迹相关变量
static trajectory_point_t g_trajectory[100];
static int g_trajectory_index = 0;
static int g_trajectory_total_points = 0;
static float g_trajectory_start_time = 0;

// 触角初始化
void Tentacle_Init(void)
{
    g_tentacle_ctrl.target.x = 0;
    g_tentacle_ctrl.target.y = 0;
    g_tentacle_ctrl.target.z = 50;
    g_tentacle_ctrl.current.x = 0;
    g_tentacle_ctrl.current.y = 0;
    g_tentacle_ctrl.current.z = 50;
    g_tentacle_ctrl.mode = TENTACLE_MODE_POSITION;
    g_tentacle_ctrl.enable = 0;
    g_tentacle_ctrl.max_speed = 50.0f;  // 50mm/s
    g_tentacle_ctrl.trajectory_time = 5.0f;  // 5秒
    g_tentacle_ctrl.circle_radius = 30.0f;
    g_tentacle_ctrl.spiral_radius = 25.0f;
    g_tentacle_ctrl.spiral_height = 100.0f;
    
    // 初始化线长度
    Tentacle_Inverse_Kinematics(g_tentacle_ctrl.current.x, g_tentacle_ctrl.current.y, g_tentacle_ctrl.current.z,
                               &g_tentacle_ctrl.current.line1_length, 
                               &g_tentacle_ctrl.current.line2_length, 
                               &g_tentacle_ctrl.current.line3_length);
    
    g_tentacle_ctrl.target.line1_length = g_tentacle_ctrl.current.line1_length;
    g_tentacle_ctrl.target.line2_length = g_tentacle_ctrl.current.line2_length;
    g_tentacle_ctrl.target.line3_length = g_tentacle_ctrl.current.line3_length;
}

// 设置目标位置
void Tentacle_Set_Target_Position(float x, float y, float z)
{
    if (!Tentacle_Is_Position_Reachable(x, y, z)) {
        DEBUG("Tentacle: Target position not reachable\n");
        return;
    }
    
    g_tentacle_ctrl.target.x = x;
    g_tentacle_ctrl.target.y = y;
    g_tentacle_ctrl.target.z = z;
    
    // 计算目标线长度
    Tentacle_Inverse_Kinematics(x, y, z, 
                               &g_tentacle_ctrl.target.line1_length,
                               &g_tentacle_ctrl.target.line2_length,
                               &g_tentacle_ctrl.target.line3_length);
}

// 设置控制模式
void Tentacle_Set_Mode(tentacle_mode_t mode)
{
    g_tentacle_ctrl.mode = mode;
    
    if (mode == TENTACLE_MODE_CIRCLE) {
        Tentacle_Generate_Circle_Trajectory(g_tentacle_ctrl.circle_radius, 
                                           g_tentacle_ctrl.current.z, 50, g_trajectory);
        g_trajectory_total_points = 50;
        g_trajectory_index = 0;
    }
    else if (mode == TENTACLE_MODE_SPIRAL) {
        Tentacle_Generate_Spiral_Trajectory(g_tentacle_ctrl.spiral_radius, 
                                           g_tentacle_ctrl.spiral_height, 3, 100, g_trajectory);
        g_trajectory_total_points = 100;
        g_trajectory_index = 0;
    }
}

// 使能触角控制
void Tentacle_Enable(uint8_t enable)
{
    g_tentacle_ctrl.enable = enable;
    if (!enable) {
        Tentacle_Stop();
    }
}

// 停止触角运动
void Tentacle_Stop(void)
{
    // 停止所有电机
    Motion_Set_Speed(0, 0, 0, 0);
    g_tentacle_ctrl.enable = 0;
}

// 正运动学：根据线长度计算触角末端位置
void Tentacle_Forward_Kinematics(float line1_len, float line2_len, float line3_len, 
                                 float* x, float* y, float* z)
{
    // 三球面相交求交点
    float x1 = ANCHOR1_X, y1 = ANCHOR1_Y, z1 = ANCHOR1_Z;
    float x2 = ANCHOR2_X, y2 = ANCHOR2_Y, z2 = ANCHOR2_Z;
    float x3 = ANCHOR3_X, y3 = ANCHOR3_Y, z3 = ANCHOR3_Z;
    
    float r1 = line1_len, r2 = line2_len, r3 = line3_len;
    
    // 使用三球面相交算法
    // 计算三个球面的交点
    
    // 计算两个球面的交线
    float a = 2*(x2-x1);
    float b = 2*(y2-y1);
    float c = 2*(z2-z1);
    float d = r1*r1 - r2*r2 - x1*x1 + x2*x2 - y1*y1 + y2*y2 - z1*z1 + z2*z2;
    
    // 计算第三个球面与交线的交点
    float e = 2*(x3-x1);
    float f = 2*(y3-y1);
    float g = 2*(z3-z1);
    float h = r1*r1 - r3*r3 - x1*x1 + x3*x3 - y1*y1 + y3*y3 - z1*z1 + z3*z3;
    
    // 求解线性方程组
    // 这里使用简化的解析解，实际应用中可能需要更复杂的数值解法
    float det = a*(b*g - c*f) - b*(a*g - c*e) + c*(a*f - b*e);
    
    if (fabs(det) < 1e-6) {
        // 方程组无解或无穷解，使用简化方法
        *x = (x1 + x2 + x3) / 3;
        *y = (y1 + y2 + y3) / 3;
        *z = sqrt((r1 + r2 + r3) / 3);  // 简化估计
    } else {
        // 求解交点
        *x = (d*(b*g - c*f) - h*(b*g - c*f)) / det;
        *y = (a*(h*g - c*d) - e*(a*g - c*d)) / det;
        *z = (a*(b*h - f*d) - b*(a*h - e*d)) / det;
    }
}

// 逆运动学：根据触角末端位置计算线长度
void Tentacle_Inverse_Kinematics(float x, float y, float z, 
                                 float* line1_len, float* line2_len, float* line3_len)
{
    // 计算到三个锚点的距离
    *line1_len = sqrt((x - ANCHOR1_X)*(x - ANCHOR1_X) + 
                      (y - ANCHOR1_Y)*(y - ANCHOR1_Y) + 
                      (z - ANCHOR1_Z)*(z - ANCHOR1_Z));
                      
    *line2_len = sqrt((x - ANCHOR2_X)*(x - ANCHOR2_X) + 
                      (y - ANCHOR2_Y)*(y - ANCHOR2_Y) + 
                      (z - ANCHOR2_Z)*(z - ANCHOR2_Z));
                      
    *line3_len = sqrt((x - ANCHOR3_X)*(x - ANCHOR3_X) + 
                      (y - ANCHOR3_Y)*(y - ANCHOR3_Y) + 
                      (z - ANCHOR3_Z)*(z - ANCHOR3_Z));
}

// 生成圆形轨迹
void Tentacle_Generate_Circle_Trajectory(float radius, float height, int num_points, 
                                         trajectory_point_t* trajectory)
{
    for (int i = 0; i < num_points; i++) {
        float angle = 2.0f * 3.14159f * i / num_points;
        trajectory[i].x = radius * cos(angle);
        trajectory[i].y = radius * sin(angle);
        trajectory[i].z = height;
        trajectory[i].time = (float)i / num_points * g_tentacle_ctrl.trajectory_time;
    }
}

// 生成螺旋轨迹
void Tentacle_Generate_Spiral_Trajectory(float radius, float height, int turns, int num_points, 
                                         trajectory_point_t* trajectory)
{
    for (int i = 0; i < num_points; i++) {
        float angle = 2.0f * 3.14159f * turns * i / num_points;
        float progress = (float)i / num_points;
        
        trajectory[i].x = radius * cos(angle) * (1 - progress);
        trajectory[i].y = radius * sin(angle) * (1 - progress);
        trajectory[i].z = height * progress;
        trajectory[i].time = (float)i / num_points * g_tentacle_ctrl.trajectory_time;
    }
}

// 生成直线轨迹
void Tentacle_Generate_Line_Trajectory(float start_x, float start_y, float start_z,
                                       float end_x, float end_y, float end_z, int num_points,
                                       trajectory_point_t* trajectory)
{
    for (int i = 0; i < num_points; i++) {
        float t = (float)i / (num_points - 1);
        trajectory[i].x = start_x + (end_x - start_x) * t;
        trajectory[i].y = start_y + (end_y - start_y) * t;
        trajectory[i].z = start_z + (end_z - start_z) * t;
        trajectory[i].time = t * g_tentacle_ctrl.trajectory_time;
    }
}

// 触角控制主处理函数
void Tentacle_Control_Handle(void)
{
    if (!g_tentacle_ctrl.enable) return;
    
    // 更新当前位置
    Tentacle_Update_Current_Position();
    
    switch (g_tentacle_ctrl.mode) {
        case TENTACLE_MODE_POSITION:
            Tentacle_Position_Control();
            break;
        case TENTACLE_MODE_TRAJECTORY:
        case TENTACLE_MODE_CIRCLE:
        case TENTACLE_MODE_SPIRAL:
            Tentacle_Trajectory_Control();
            break;
        default:
            break;
    }
}

// 位置控制
void Tentacle_Position_Control(void)
{
    // 计算线长度误差
    float line1_error = g_tentacle_ctrl.target.line1_length - g_tentacle_ctrl.current.line1_length;
    float line2_error = g_tentacle_ctrl.target.line2_length - g_tentacle_ctrl.current.line2_length;
    float line3_error = g_tentacle_ctrl.target.line3_length - g_tentacle_ctrl.current.line3_length;
    
    // 限制速度
    float max_error = g_tentacle_ctrl.max_speed * 0.01f;  // 10ms周期
    if (line1_error > max_error) line1_error = max_error;
    if (line1_error < -max_error) line1_error = -max_error;
    if (line2_error > max_error) line2_error = max_error;
    if (line2_error < -max_error) line2_error = -max_error;
    if (line3_error > max_error) line3_error = max_error;
    if (line3_error < -max_error) line3_error = -max_error;
    
    // 转换为电机速度 (mm/s)
    float motor1_speed = line1_error * 100.0f;  // 转换为mm/s
    float motor2_speed = line2_error * 100.0f;
    float motor3_speed = line3_error * 100.0f;
    
    // 设置电机目标速度
    PID_Set_Motor_Target(MOTOR_ID_M1, motor1_speed);
    PID_Set_Motor_Target(MOTOR_ID_M2, motor2_speed);
    PID_Set_Motor_Target(MOTOR_ID_M3, motor3_speed);
}

// 轨迹控制
void Tentacle_Trajectory_Control(void)
{
    if (g_trajectory_index >= g_trajectory_total_points) {
        g_trajectory_index = 0;  // 循环轨迹
    }
    
    // 设置当前轨迹点为目标位置
    Tentacle_Set_Target_Position(g_trajectory[g_trajectory_index].x,
                                 g_trajectory[g_trajectory_index].y,
                                 g_trajectory[g_trajectory_index].z);
    
    // 检查是否到达当前轨迹点
    float distance = Tentacle_Calculate_Distance(g_tentacle_ctrl.current.x, g_tentacle_ctrl.current.y, g_tentacle_ctrl.current.z,
                                                g_tentacle_ctrl.target.x, g_tentacle_ctrl.target.y, g_tentacle_ctrl.target.z);
    
    if (distance < 5.0f) {  // 5mm容差
        g_trajectory_index++;
    }
    
    // 执行位置控制
    Tentacle_Position_Control();
}

// 计算两点间距离
float Tentacle_Calculate_Distance(float x1, float y1, float z1, float x2, float y2, float z2)
{
    return sqrt((x2-x1)*(x2-x1) + (y2-y1)*(y2-y1) + (z2-z1)*(z2-z1));
}

// 更新当前位置
void Tentacle_Update_Current_Position(void)
{
    // 从编码器获取当前线长度
    // 这里需要根据实际的编码器到线长度的转换关系来实现
    // 假设编码器脉冲直接对应线长度变化
    
    // 获取编码器数据
    int encoder_now[4];
    Encoder_Get_ALL(encoder_now);
    
    // 计算线长度变化（这里需要根据实际机械结构调整）
    static int encoder_last[4] = {0};
    float line1_delta = (encoder_now[0] - encoder_last[0]) * TENTACLE_PULSE_TO_MM;
    float line2_delta = (encoder_now[1] - encoder_last[1]) * TENTACLE_PULSE_TO_MM;
    float line3_delta = (encoder_now[2] - encoder_last[2]) * TENTACLE_PULSE_TO_MM;
    
    // 更新线长度
    g_tentacle_ctrl.current.line1_length += line1_delta;
    g_tentacle_ctrl.current.line2_length += line2_delta;
    g_tentacle_ctrl.current.line3_length += line3_delta;
    
    // 限制线长度范围
    if (g_tentacle_ctrl.current.line1_length > TENTACLE_MAX_LINE_LENGTH) 
        g_tentacle_ctrl.current.line1_length = TENTACLE_MAX_LINE_LENGTH;
    if (g_tentacle_ctrl.current.line1_length < TENTACLE_MIN_LINE_LENGTH) 
        g_tentacle_ctrl.current.line1_length = TENTACLE_MIN_LINE_LENGTH;
        
    if (g_tentacle_ctrl.current.line2_length > TENTACLE_MAX_LINE_LENGTH) 
        g_tentacle_ctrl.current.line2_length = TENTACLE_MAX_LINE_LENGTH;
    if (g_tentacle_ctrl.current.line2_length < TENTACLE_MIN_LINE_LENGTH) 
        g_tentacle_ctrl.current.line2_length = TENTACLE_MIN_LINE_LENGTH;
        
    if (g_tentacle_ctrl.current.line3_length > TENTACLE_MAX_LINE_LENGTH) 
        g_tentacle_ctrl.current.line3_length = TENTACLE_MAX_LINE_LENGTH;
    if (g_tentacle_ctrl.current.line3_length < TENTACLE_MIN_LINE_LENGTH) 
        g_tentacle_ctrl.current.line3_length = TENTACLE_MIN_LINE_LENGTH;
    
    // 更新触角末端位置
    Tentacle_Forward_Kinematics(g_tentacle_ctrl.current.line1_length,
                               g_tentacle_ctrl.current.line2_length,
                               g_tentacle_ctrl.current.line3_length,
                               &g_tentacle_ctrl.current.x,
                               &g_tentacle_ctrl.current.y,
                               &g_tentacle_ctrl.current.z);
    
    // 更新各节的位置和姿态
    Tentacle_Update_Segment_Positions(g_tentacle_ctrl.current.segments);
    
    // 保存当前编码器值
    for (int i = 0; i < 4; i++) {
        encoder_last[i] = encoder_now[i];
    }
}

// 检查位置是否可达
uint8_t Tentacle_Is_Position_Reachable(float x, float y, float z)
{
    float line1_len, line2_len, line3_len;
    Tentacle_Inverse_Kinematics(x, y, z, &line1_len, &line2_len, &line3_len);
    
    // 检查线长度是否在合理范围内
    if (line1_len < TENTACLE_MIN_LINE_LENGTH || line1_len > TENTACLE_MAX_LINE_LENGTH ||
        line2_len < TENTACLE_MIN_LINE_LENGTH || line2_len > TENTACLE_MAX_LINE_LENGTH ||
        line3_len < TENTACLE_MIN_LINE_LENGTH || line3_len > TENTACLE_MAX_LINE_LENGTH) {
        return 0;
    }
    
    return 1;
}

// 多节触角运动学函数实现

// 计算各节的位置和姿态
void Tentacle_Calculate_Segments(float line1_len, float line2_len, float line3_len,
                                 tentacle_segment_t* segments)
{
    // 计算触角末端位置
    float end_x, end_y, end_z;
    Tentacle_Forward_Kinematics(line1_len, line2_len, line3_len, &end_x, &end_y, &end_z);
    
    // 计算触角的总弯曲角度
    float total_angle = atan2(sqrt(end_x*end_x + end_y*end_y), end_z);
    float total_angle_x = atan2(end_y, end_x);
    
    // 计算每节的弯曲角度
    float segment_angle = total_angle / TENTACLE_SEGMENTS;
    float segment_angle_x = total_angle_x / TENTACLE_SEGMENTS;
    
    // 计算每节的位置
    for (int i = 0; i < TENTACLE_SEGMENTS; i++) {
        float progress = (float)(i + 1) / TENTACLE_SEGMENTS;
        
        // 计算当前节的位置
        segments[i].x = end_x * progress;
        segments[i].y = end_y * progress;
        segments[i].z = end_z * progress;
        
        // 计算当前节的半径（锥形递减）
        segments[i].radius = TENTACLE_BASE_RADIUS * pow(TENTACLE_TAPER_RATIO, i);
        
        // 计算当前节的倾斜角度
        segments[i].angle_x = segment_angle_x * (i + 1);
        segments[i].angle_y = segment_angle * (i + 1);
    }
}

// 更新各节的位置
void Tentacle_Update_Segment_Positions(tentacle_segment_t* segments)
{
    // 计算触角末端位置
    float end_x, end_y, end_z;
    Tentacle_Forward_Kinematics(g_tentacle_ctrl.current.line1_length,
                               g_tentacle_ctrl.current.line2_length,
                               g_tentacle_ctrl.current.line3_length,
                               &end_x, &end_y, &end_z);
    
    // 更新各节位置
    Tentacle_Calculate_Segments(g_tentacle_ctrl.current.line1_length,
                               g_tentacle_ctrl.current.line2_length,
                               g_tentacle_ctrl.current.line3_length,
                               segments);
}

// 获取指定节的半径
float Tentacle_Get_Segment_Radius(int segment_index)
{
    if (segment_index < 0 || segment_index >= TENTACLE_SEGMENTS) {
        return 0.0f;
    }
    
    return TENTACLE_BASE_RADIUS * pow(TENTACLE_TAPER_RATIO, segment_index);
}
